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(57) ABSTRACT 

A method of stabilizing an inertial navigation system (INS), 
includes the steps of: receiving data from an inertial naviga- 
tion system; and receiving a finite number of carrier phase 
observables using at least one GPS receiver from a plurality of 
GPS satellites; calculating a phase wind up correction; cor- 
recting at least one of the finite number of carrier phase 
observables using the phase wind up correction; and calcu- 
lating a corrected IMU attitude or velocity or position using 
the corrected at least one of the finite number of carrier phase 
observables; and performing a step selected from the steps 
consisting of recording, reporting, or providing the corrected 
IMU attitude or velocity or position to another process that 
uses the corrected IMU attitude or velocity or position. A GPS 
stabilized inertial navigation system apparatus is also 
described. 
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GPS/INS SENSOR FUSION USING GPS WIND 
UP MODEL 

CROSS-REFERENCE TO RELATED 

APPLICATIONS 5 

This application claims priority to and the benefit of co- 
pending U.S. provisional patent application Ser. No. 61/382, 
333, filed Sep. 13, 2010, which application is incorporated 
herein by reference in its entirety. 10 

STATEMENT REGARDING FEDERALLY 
FUNDED RESEARCH OR DEVELOPMENT 

The invention described herein was made in the perfor- 15 
mance of work under a NASA contract, and is subject to the 
provisions of Public Law 96-517 (35 USC 202) in which the 
Contractor has elected to retain title. 

FIELD OF THE INVENTION 20 

The invention relates to inertial navigation in general and 
particularly to an inertial navigation system that employs a 
GPS correction of inertial navigation system errors. 

25 

BACKGROUND OF THE INVENTION 

Methods for compensating inertial navigation with GPS 
have been described, for example, by Williamson, et. al., in 
“Sensor Fusion Applied to Autonomous Aerial Refueling, 30 
Journal of Guidance, Control, and Dynamics,” Vol. 32, No. 1 , 
January-February 2009, Britting, in Inertial Navigation Sys- 
tems Analysis, Wiley-Interscience, New York, 1971, and S. 
Hong, et. al., in Observability Analysis of GPS Aided INS, 
Proceedings of the 2000 Institute of Navigation, Salt Lake 35 
City, Utah, September 2000. Also, recent advances in GPS 
technology such as have been described by Y. E. Bar-Sever, et. 
al., in “Estimating horizontal gradients of tropospheric path 
delay with a single GPS receiver,” Journal of Geophysical 
Research, Vol. 103, no. B3, pages 5019-5035, Mar. 10, 1998, 40 
describe the ability to resolve integer ambiguity between a 
GPS receiver and the GPS satellite. The ability to resolve 
integer ambiguity between a GPS receiver and the GPS sat- 
ellite can be used to increase position accuracy, as well as to 
compensate inertial drift errors by use of GPS observables. 45 

However, most of the carrier phase observables of interest 
are affected by the “phase wind up effect” which has been 
described by Wu, et. al., in “Effects of antenna orientation on 
GPS carrier phase,” Manuscripta Geodetica Vol. 18, pages 
91-98, 1993. The phase wind up effect causes the GPS carrier 50 
phase to advance as the GPS antemia rotates. In present day 
GPS compensated INS systems, there are wind up errors such 
as wind up errors caused by caused by the rotation of a GPS 
antenna. 

There is a need for a system and method which can correct 55 
an inertial navigation system for inertial navigation system 
errors caused by carrier phase wind up. 

SUMMARY OF THE INVENTION 

60 

According to one aspect, the invention features a method of 
stabilizing an inertial navigation system (INS), which 
includes the steps of: performing the following steps in any 
order: receiving data from an inertial navigation system hav- 
ing an inertial measurement unit (IMU) ; and receiving a finite 65 
number of carrier phase observables using at least one GPS 
receiver from a plurality of GPS satellites; performing the 


2 

following series of calculations: calculating a phase wind up 
correction; correcting at least one of the finite number of 
carrier phase observables using the phase wind up correction; 
and calculating a corrected IMU attitude or velocity or posi- 
tion using the corrected at least one of the finite number of 
carrier phase observables; and performing a step selected 
from the steps consisting of recording the corrected IMU 
attitude or velocity or position, reporting the corrected IMU 
attitude or velocity to a user, and providing the corrected IMU 
attitude or velocity or position to another process that uses the 
corrected IMU attitude or velocity or position. 

In one embodiment, the method further comprises the step 
of providing the corrected IMU attitude or velocity or posi- 
tion to the INS. 

In another embodiment, the INS receives the corrected 
IMU attitude or velocity or position. 

In yet another embodiment, the step of calculating a cor- 
rected IMU attitude or velocity or position includes calculat- 
ing a corrected IMU attitude or velocity or position using the 
corrected at least one of the finite number of carrier phase 
observables with full state observability. 

In yet another embodiment, the steps of calculating a phase 
wind up correction and correcting at least one of the finite 
number of carrier phase observables using the phase wind up 
correction are performed using the following equations: 



and <|) A ,-<|) T ,=H MG£Ar H 4 , 1 H 0 8 x , where 


-FtaiT 1 ^) 

Hmgekf = y • 

and H 4>I =[sin(p i )-cos(P f )0]. 

In yet another embodiment, the steps of calculating a phase 
wind up correction and correcting at least one of the finite 
number of carrier phase observables using the phase wind up 
correction are performed using the following equations: 



and Uj-UuH^H^H^Sx, where 



In yet another embodiment, the step of receiving a finite 
number of carrier phase observables includes receiving a 
finite number of carrier phase observables using at least one 
GPS receiver from a plurality of GPS satellites using a GPS 
receiver having a plurality of GPS antennas disposed at dif- 
ferent angles with different dipole definitions in a coordinate 
reference system and wherein the step of performing the 
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following series of calculations includes calculating a phase 
wind up correction for each of the plurality of GPS antennas 
disposed at different angles. 

In yet another embodiment, the step of receiving a finite 
number of carrier phase observables includes receiving a 5 
finite number of carrier phase observables using at least one 
GPS receiver from a plurality of GPS satellites using a plu- 
rality of GPS antennas disposed substantially in a circle and 
wherein the step of performing the following series of calcu- 
lating includes calculating a phase wind up correction for to 
each of the plurality of GPS antennas disposed substantially 
in a circle. 

In yet another embodiment, the method of stabilizing an 
inertial navigation system further includes a GPS receiver 
having at least one GPS antenna which is present on a first 15 
vehicle, and a second GPS receiver having at least one GPS 
antenna is configured to perform the method of claim 1 on a 
second vehicle different from the first vehicle, and further 
including a step of using a single or double differenced carrier 
phase measurement to provide a range between the first and 20 
second vehicles. 

In yet another embodiment, the step of calculating a cor- 
rected IMU attitude or velocity or position provides an 
enhanced accuracy yaw angle. 

In yet another embodiment, the step of calculating a cor- 25 
rected IMU attitude or velocity or position includes calcula- 
tion of an Extended Kalman Filter (EKF) or of a Square Root 
Information Filter (SRIF). 

According to another aspect, the invention features a GPS 
stabilized inertial navigation system apparatus which 30 
includes at least one GPS receiver having at least one GPS 
antenna. An inertial navigation system (INS) has an inertial 
measurement unit (IMU). A data processor is communica- 
tively coupled to the at least one GPS receiver and the IMU. 
The data processor is configured to receive a plurality of GPS 35 
carrier phase observables from the GPS receiver and attitude 
and velocity data from the IMU. The data processor includes 
a module which has instructions in machine-readable fonn 
recorded therein. The data processor is configured to perform, 
when operating under the control of the instructions, a pro- 40 
cess including the steps of: calculating a phase wind up cor- 
rection; correcting at least one of the finite number of carrier 
phase observables using the phase wind up correction; and 
calculating a corrected IMU attihide or velocity or position 
using the corrected at least one of the finite number of carrier 45 
phase observables. At least one device selected from the 
group of devices including: a device configured to record the 
corrected IMU attitude or velocity or position, a device is 
configured to report the corrected IMU attitude or velocity or 
position to a user, and a device configured to provide the 50 
corrected IMU attitude or velocity or position to another 
process that uses the corrected IMU attitude or velocity or 
position. 

In one embodiment, the INS is configured to receive the 
corrected IMU attitude or velocity or position to correct an 55 
INS attitude or velocity or position. 

In another embodiment, the data processor is configured to 
calculate a corrected IMU attihide or velocity or position with 
full state observability. 

In yet another embodiment, the GPS stabilized inertial 60 
navigation system apparatus further includes a plurality of 
GPS antennas disposed at different angles. 

In yet another embodiment, the GPS stabilized inertial 
navigation system apparatus further includes a plurality of 
GPS antennas disposed substantially in a circle. 65 

In yet another embodiment, the data processor is config- 
ured to provide an enhanced accuracy yaw angle. 
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The foregoing and other objects, aspects, features, and 
advantages of the invention will become more apparent from 
the following description and from the claims. 

BRIEF DESCRIPTION OF THE DRAWINGS 

The objects and features of the invention can be better 
understood with reference to the drawings described below, 
and the claims. The drawings are not necessarily to scale, 
emphasis instead generally being placed upon illustrating the 
principles of the invention. In the drawings, like numerals are 
used to indicate like parts throughout the various views. 

FIG. 1 shows a simplified block diagram of one exemplary 
method for stabilizing an inertial navigation system accord- 
ing to the invention. 

FIG. 2 shows a simplified block diagram of one exemplary 
system for performing the method of FIG. 1 according to the 
invention. 

DETAILED DESCRIPTION 

As described hereinabove, the phase wind up effect causes 
the carrier phase to advance as the GPS antenna rotates. This 
unique signature of the resolved ambiguity phase enables the 
observability and stabilization of the inertial navigation sys- 
tem yaw and yaw bias, states that are weakly observable 
without significant and time varying inertial acceleration. 
Enabling the use of carrier phase wind up models to correct 
the inertial errors is expected to have a significant impact on 
the production and use of low cost inertial systems for scien- 
tific and real time applications, since the yaw angle will now 
have an accuracy commiserate with the pitch and roll angles. 

A system and method for using GPS carrier phase mea- 
surements to correct a navigation state generated by an iner- 
tial navigation system (INS) is described hereinbelow. In one 
exemplary embodiment, the method assumes at least one 
GPS receiver and one inertial navigation system operating on 
a single vehicle. The system and method takes advantage of a 
resolved integer ambiguity between the satellite and the GPS 
receiver in order to form precise carrier phase observables. 
These observables are processed within a filter structure such 
as the Extended Kalman Filter (EKF) to correct the INS state 
for drift due to error sources in the accelerometer and rate 
gyros. The system and method takes advantage of the carrier 
phase observable wind up effect to ensure that the complete 
attihide and the gyro bias errors are always observable, even 
when the GPS antenna is static (not physically moving). The 
inventive system and method as described hereinbelow, also 
fixes the GPS/INS navigation systems “yaw angle drift over 
time” problem, which is caused by an inability to observe yaw 
rate biases. 

GPS/INS Sensor Fusion Method Using GPS Wind Up Model 

FIG. 1 shows a simplified block diagram of one exemplary 
method 100 for stabilizing an inertial navigation system 
according to the invention. Hie steps are: performing the 
following steps in any order: ( 103 ) receiving data from an 
inertial navigation system having an inertial measurement 
unit (IMU); ( 101 ) receiving a finite number of carrier phase 
observables using at least one GPS receiver from a plurality of 
GPS satellites; and then ( 107 ) perform the following series of 
calculations: calculate a phase wind up correction; correct at 
least one of the finite number of carrier phase observables 
using the phase wind up correction; and calculate a corrected 
IMU attihide or velocity or position using the corrected at 
least one of the finite number of carrier phase observables; 
and ( 109 ) perform a step selected from the steps consisting of 
recording the corrected IMU attitude or velocity or position, 
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reporting the corrected IMU attitude or velocity or position to 
a user, and providing the corrected IMU attitude or velocity or 
position to another process that uses the corrected IMU atti- 
tude or velocity or position. In one embodiment, the corrected 
IMU attitude or velocity or position is provided back to the 
INS (i.e. the INS receives the corrected IMU attitude or veloc- 
ity or position). 

As described in more detail below, the method can be 
performed with full state observability using only one GPS 
antenna or when using a plurality of GPS antennas. In some 
embodiments, the method 100 of stabilizing an inertial navi- 
gation system can be used to provide an enhanced accuracy 
yaw angle such as for a vehicle attitude (e.g. an aircraft, car, 
truck, or other type of vehicle). 

In one embodiment, performing calculations 107 includes 
calculating a phase wind up correction and correcting at least 
one of the finite number of carrier phase observables using the 
phase wind up correction. The calculations are performed 
using the following equations: 



and ^,-^^mgerf^^dK where 

-Ftan-'m 
Hmgekf = q ; , 

and H^tsin^J-cosl^iyO]. In another embodiment, per- 
forming calculations 107 includes calculating a phase wind 
up correction and correcting at least one of the finite number 
of carrier phase observables using the phase wind up correc- 
tion. The calculations are performed using the following 
equations: 



and U rk =H 4 H s H d 8x, where 


Z U y 

H a = T — , and 

(Dy\ D x 

i+ u 

D x 


In some embodiments, providing a GPS receiver with a 
GPS antenna 101 includes providing a plurality of GPS anten- 
nas disposed at different angles with different dipole defini- 
tions in a coordinate reference system. In other embodiments, 
providing a GPS receiver with a GPS antenna 101 includes 
providing a plurality of GPS antennas disposed substantially 
in a circle. 

There can also be embodiments of the invention where 
there are two different systems that can perform the steps 100, 
each system being disposed in a different location, such as, 
for example, each system mounted in or on a different vehicle. 
In such systems, a single or double differenced carrier phase 
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measurement can be used to provide a range between the two 
locations, such as to provide a range measurement between 
two vehicles. 

GPS/INS Sensor Fusion System Using GPS Wind Up Model 
5 FIG. 2 shows a simplified block diagram of one exemplary 

system which is configured to perform the method of FIG. 1 
according to the invention. A GPS stabilized inertial naviga- 
tion system apparatus 200 includes at least one GPS receiver 
205 having at least one GPS antenna 207. An inertial naviga- 
to tion system 201 has an inertial measurement unit 203 (IMU). 
A data processor 209, such as a general purpose program- 
mable computer, is communicatively coupled by (via com- 
munications link 223) to the at least one GPS receiver 205 and 
communicatively coupled (via communications link 221) to 
15 the INS 201 and/or the IMU 203. The data processor 209 is 
configured to receive a plurality of GPS carrier phase observ- 
ables from the GPS receiver 205 and attitude and velocity data 
from the INS 201 and/or the IMU 203 . The data processor 209 
includes a module having instructions in machine-readable 
20 form recorded therein. The data processor 209 is configured 
to perform, when operating under the control of the instruc- 
tions, a process including the steps of: calculating a phase 
wind up correction; correcting at least one of the finite num- 
ber of carrier phase observables using the phase wind up 
25 correction; and calculating a corrected IMU attitude or veloc- 
ity or position using the corrected at least one of the finite 
number of carrier phase observables; and at least one device 
selected from the group of devices consisting of a device 
configured to record the corrected IMU attitude or velocity or 
30 position, a device configured to report the corrected IMU 
attitude or velocity or position to a user, and a device config- 
ured to provide the corrected IMU attitude or velocity or 
position to another process that uses the corrected IMU atti- 
tude or velocity or position. Data processor 209 can be com- 
35 municatively coupled (via communications link 225) to a 
recording device, a display device, and/or configured for data 
communications with another device or system 211. The GPS 
stabilized inertial navigation system apparatus 200 can cal- 
culate a corrected IMU attitude or velocity or position with 
40 Hill state observability. The GPS stabilized inertial navigation 
system apparatus 200 can be configured to provide an 
enhanced accuracy yaw angle, such as an enhanced accuracy 
yaw angle of an aircraft, car, truck, or other type of vehicle. 

Communication links 221, 223, and 225 are typically hard 
45 wired data communication links such as wires, cables and/or 
standard data bus connections. However, as can be appreci- 
ated by those skilled in the art, any suitable communication 
link can be used including for example, fiber optic links, or 
any other suitable wired or wireless communication link 
50 Typically, each corrected IMU attitude or velocity or position 
is also reported back to the INS to correct a present INS 
attitude or velocity or position value (not shown in FIG. 1 and 
FIG. 2). 

In some embodiments (not shown in FIG. 2), the GPS 
55 antenna 207 can include a plurality of GPS antennas disposed 
at different angles. In other embodiments, the GPS antenna 
207 can also include a plurality of GPS antennas disposed 
substantially in a circle. In embodiments using multiple GPS 
antennas, typically there is at least one GPS receiver section 
60 electrically coupled to each GPS antenna. 

Inertial Navigation and Error Models 

The primary instrument of an inertial navigation system 
(INS) is an inertial measurement unit (IMU). An IMU typi- 
cally, but not necessarily, includes 3 gyros and 3 accelerom- 
65 eter sensors. Each of the three sensors is aligned along a 
respective one of an orthogonal set of axes. The IMU is 
usually disposed on or in a vehicle or other device or platform 
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where the user wants to estimate at least one of the position, 
velocity, or attitude of the device or platform. Given an initial 
position, velocity, and attitude, it is possible to integrate the 
angular rate gyro measurements over time to update the 
vehicle attitude. Using the estimate of attitude, it is possible to 5 
estimate the direction of sensed gravity and remove it from 
the accelerometer measurements leaving only vehicle accel- 
eration. The vehicle acceleration can then be integrated to 
update the velocity and the velocity integrated to update the 
position. to 

The method described in this section is similar to the meth- 
ods described by Hong, et. al., in “Observability Analysis of 
GPS Aided INS,” Proceedings of the 2000 Institute of Navi- 
gation, Salt Lake City, Utah, September 2000, and by Will- 
iamson, et. al., in “Sensor Fusion Applied to Autonomous 15 
Aerial Refueling,” Journal of Guidance, Control, and Dynam- 
ics, Vol. 32, No. 1, January-February 2009, although the 
navigation calculations performed herein, are done in an 
Earth Centered Earth Fixed (ECEF) reference frame in order 
to more easily integrate the navigation calculations with the 20 
GPS measurement equations. 

The method of using an IMU to update a navigation state 
can be defined by the “strap-down” equations of motion, 
named for the fact that the equations assume a solid state IMU 
that can be “strapped down” to a vehicle as opposed to mov- 25 
ing mass and gimbaled types of gyros. However, the method 
and apparatus of the instant invention can be applied to any 
source of angular rate and acceleration data and is not 
restricted to solid state instruments. 

Inertial Measurements 30 

Typical IMU measurements include measurements of 
acceleration and angular velocity. An alternative, more com- 
mon among navigation grade or better inertial systems, is to 
provide a measurement of change in velocity (Av) and change 
in angle (A0). If the inertial acceleration of the vehicle is 35 
defined as a IB B where the body frame acceleration is mea- 
sured relative to the inertial frame in the vehicle body frame, 
then the delta velocity over a time interval At is defined as: 

& v IB B= fo A ‘ a IB B dt (1 ) 4Q 

Similarly, the delta theta is defined as the integration of the 
instantaneous angular velocity va IB over the time interval At: 

ASi B B =J 0 A ‘(B IB B dt ( 2 ) 

This analysis described hereinbelow uses Av’s and AO’s, 45 
and assumes that body frame orientation errors are small 
relative to the sample time interval At. 

Inertial measurements are subject to errors. For three pur- 
poses of the present description, these errors are lumped into 
an additive error referred to as the bias error. Any other 50 
suitable errors, such as for example, scale factor errors, mis- 
alignment errors, and temperature dependent errors can also 
be used. The measured change in velocity Av IB B is equal to the 
true change in velocity Av IB B plus the bias term b Av in Eq. 3. 

In this case, the true bias b Av is assumed constant over the 55 
interval At. 

AV//“A v m B +b^At (3) 

Likewise, a similar bias error is defined for the angular rate 
gyro measurements AQ IB B relative to the true value AQ IB B : 60 

A@ JB B =A®i B B +b A& At (4) 

If known calibrations exist, they can be applied at each time 
interval. The errors modeled here account for the remaining 
un-calibrated error sources. One of our goals is to develop a 65 
filter using GPS measurement to estimate errors in the navi- 
gation state as well as errors in the calibration of the errors in 
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the instruments. We define both of the true biases in terms of 
the estimate plus an unknown perturbation error to be esti- 
mated: 


^ Av ^Av"*"^^Av 

(5) 

b A0 = ^A0 + ^ A© 

(6) 


In this case, the a priori estimate of the accelerometer bias 
is b Av and the uncertainty in the bias to be estimated is 8b Av . 
Similarly, the estimate of the bias in the rate gyros is referred 
to as b Ae and the the uncertainty to be estimated is defined as 
8b A0 . 

The dynamics of the uncertainty are defined as a random 
walk with time constants x Av and x A0 driven by independent, 
zero mean Gaussian noise processes v Av and v A0 . 


6b, 


v (f + Af) = ft - — WvW + >' 4v 
V t Av I 


6t>Bo(t + A/) 


1 tab) 


6b A Q{t) + v AQ 


(7) 

( 8 ) 


Strap Down Equations of Motion 

The strap down equations of motion can be used to perform 
the following tasks: to remove the effects of Earth angular 
velocity from the angular velocity measurements, to integrate 
the angular velocity to update the vehicle attitude, to remove 
the effects of centripetal and coriolis acceleration from the 
acceleration measurements, to remove the effects of gravity 
from the acceleration measurements, to integrate the accel- 
eration measurements to update the velocity estimate, and to 
integrate the velocity estimate to update the vehicle position. 

The strap down equations of motion use an initial condition 
of the position, velocity and attitude. The initial consition can 
be estimated or provided using any suitable means, such as for 
example, by using a GPS only solution to provide position 
and velocity and a gyro-alignment technique to estimate an 
initial attitude. 

The state of the vehicle x is defined as a vector including the 
following parameters: 



CT 


(9) 


P 5 is the position in the ECEF coordinate frame, V E is the 
velocity estimate in the ECEF coordinate frame and y B E is the 
(3x1) vector estimate of the Euler angles representing the 
rotation from the vehicle body frame to the ECEF frame. The 
estimates of the errors in the inertial system E Av and b A0 are 
included to estimate the errors in these terms as part of the 
filtering process. In addition, using raw pseudorange and 
carrier phase GPS measurements to correct the filter, a scalar 
clock bias error for the GPS receiver clock bias, cx is 
included, where c is the speed of light and x is the clock bias 
in seconds. A first order, stochastic model is used to simplify 
the notation, however higher order models can easily be 
employed. It is implicitly assumed that the IMU measure- 
ments are synchronized in some way to the GPS measure- 
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ments so that there is effectively only one clock. Any suitable 


scheme including a means for propagating the navigation 


1 0 

0 

COS((503>) 

0 sin((50y) ’ 

state relative to GPS can be used. 

c£( Af) = 

0 cos(<50* ) 

sin((50 x ) 

0 

1 0 

The vector y is defined in Eq. 1 0 in which 4> is the roll angle 
(rotation about the x axis), 0 is the pitch angle (rotation about 

0 -sin(<50 x ) 

cos((50 x ) 

-sin^©^) 

0 cos(<5© y ) 



( 15 ) 


the y angle) and ip is the yaw angle (rotation about z axis). In 
this description, an aircraft coordinate frame is used in which 
the x axis points to fore, the y axis points to starboard, and the 
z axis points down. Note that the (3x3) rotation matrix C B E is 
constructed from y as shown in Eq. 1 1 and Eq. 12 where it is 
noted that the Euler angles explicitly define the rotation of the 
body frame relative to the local North-East-Down (NED) 
reference frame. This rotation is represented by C B NED . In 
addition, the rotation from the NED to the ECEF frame is 
defined as C' AED ' E and this rotation matrix is purely a function 
of the vehicle ECEF position and the Earth model utilized 
(WGS-84, for instance). The Euler rotation order is useful for 
compatibility with an aircraft and because the ip angle is 
coincident with heading (angle relative to North) in most 
terrestrial applications. Rotations can be equivalently defined 
using quaternions, although this is ignoredhere for the sakeof 
brevity and notation. Suitable such quaternions have been 
described by Zipfel, in “Modeling and Simulation of Aero- 
space Vehicle Dynamics,” AIAA Press, 2000, pp. 121-128. 
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sin(0) 

0 

1 

0 

0 

-sin (0) 

o 

o 

-sin(0) 

0 

cos(0) 


cos (ij/) sin(i j/) 0 
-sin(f/r) cos(i jj) 0 
0 0 1 
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The strap down equations of motion proceed as follows. 
First, the attitude is updated. The corrected h® EB B which 
angular rotation of the body frame relative to the ECEF frame 
in the body frame is formed from the available measurements 


cos((50 z ) sin(<50 z ) 0 
-sin(<50 z ) cos(<50 z ) 0 
0 0 1 


Once the attitude is updated, the attitude is used to help 
remove the affects of gravity ^ as well as centripetla and 
Coriolis acceleration due to the Earth’s rotation. The updated 
&v EB B is defined in Eq. 1 6 . In this case the instantaneous Earth 
gravity is given as g 5 in the ECEF frame, and the notation 
l ai /£ £x ] represents the cross product matrix representation of 


the vector u> lE as shown in Eq. 1 7. 


25 


Arfg — - g E At - [tof E x][tof E x]F^At - 2o)f E x V A t 

(17) 

I to,, II -to, 0 to, I 

[*V/£ x] = 


OJ x ' 

0 

(O z 

-° J Z 

OJy 

-OJ z 

0 

a> x 


COy 

-co x 

0 


30 From this point a straight Euler type of integration can be used 
to estimate velocity and position: 


35 


!*(»+. At)=l £ (t)+Av EB E 
~P E {t+M)=T E (t)+T E (t+\t)\t 


(18) 


(19) 


Any other such suitable methods can be used to improve 
the numerical performance, such as, for example, Runge- 
Kutta integration schemes. 

Perturbations and Error Propagation 

This section describes a linearized propagation equation 
for how the unknown errors in the acceleration and angular 
rate 6b v and 6b e propagate into errors in the navigation state. 
The perturbations and error propagation section ends with a 
stochastic dynamics model for the error propagation suitable 
for use in the dynamics of an Extended Kalman Filter. 

Linear perturbations are defined in each of the state vari- 
ables. The perturbations define the true values relative to the 
estimates plus an unknown error to be estimated. 


50 


p E =p E +bP E 


( 20 ) 


integrated in time. 


v E =r E +br E 


(21) 


A 0 )/.;yj Vac-)///*- ( ( ( 'V +H A0 )a t ( 13 ) 

Hie A& eb b vector represents a small angle rotation of the 
vehicle body frame from time t to time t+At. These angles are 
in the body coordinate frame. A cosine rotation matrix is 
formed from the angles, referred to as Cg_ + (At), and is used 
to update the attitude of the vehicle. 

C B E (t+At)=C B E (tjC B B *(At) ( 14 ) 

If we define A0 £5 ' B =[60 T 80 v 60J r , then the rotation 
matrix C s _' s+ (At) is defined as in Eq. 15. In this case it is 
assumed that the z axis rotation is first, followed by y, then x, 
preserving the heading-pitch-roll ordering of the rotations. 


The linearized error in the rotation matrix Cg £ nonlinear in 
structure. The definition of the rotation error is defined in Eq. 
22. In this equation, the I matrix is the identity matrix. The 
vector by is defined as the linearized error in the Euler angles 
y used to construct C B E using Eq. 1 1 . 

C s £ =C/(/+[&yx]) (22) 

The clock bias term ex is assumed to have a white noise 
model as shown in Eq. 23. 

cx=ct+cbz (23) 

The clock bias error is driven by a discrete time stochastic 
process as shown in Eq. 24. 

c6r(f+Af)=dk(f)+v x 


55 


60 


65 


(24) 
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11 

The IMU bias perturbations were defined in Eq. 5 and Eq. 

6 with dynamics defined in Eq. 7 and Eq. 8, respectively. 

Given these definitions, an error state 5x is defined as the 
vector composed of the errors in the position, velocity, atti- v = 

tude, accelerometers, and rate gyros, as shown in Eq. 25: 5 


Sf * 
SV E 


6x = 


6y 

6b Av 

6b AQ 


The noise values v Av , v A0 , and v T are here modeled as zero 
mean, white noise processes. Each one is a 3x1 vector except 
to which is a scalar. The vector v is therefore 7x1 . Given an 
initial uncertainty M=E[8x r 8x], the covariance propagates 
forward in time using Eq. 30 with process noise W=E[v r v]. 


c6t 


The stochastic, discrete time propagation equation is given 
in Eq. 26. It was generated by substitution of the perturbation 
errors into the strap down equations of motion and eliminat- 
ing terms with order greater than one. In this case, the state 
transition matrix <E(t,t+At) is defined in Eq. 27. A stochastic 
error term T(t,t+At) is defined in Eq. 28 where the stochastic 
error v is assumed to include the random error in the accel- 
erometers, angular rate measurements, and the clock bias. 
The vector v is defined in Eq. 29. 


P(l+M)=<S>(t,t+M)M<S>'(t,t+M)+r(t,t+&t) WT T (t,t+At) (30) 

Other error models for the inertial measurements are pos- 
sible such as using misalignment, scale factor, or temperature 
effects adding states as appropriate. Higher order clock mod- 
eling can also be employed. 

The GPS measurement correction model used to estimate 
the error state 8x defined in this section using an Extended 
Kalman Filter is described next in the GPS Correction of 
Inertial Errors section. 

GPS Correction of Inertial Errors 

This section describes the GPS measurement equation for 
a variety of frequencies and observables. The carrier phase 


15 


20 


dx(i + Ar) = 4>(r, t + At)6x(t) + Tfr, r + Ar)v 


0(r, t + At) « I l6xl6 + AAt 


0 6x3 

-([4x][«4xl -G E ) -2«x] 
0 0 

-5 = 0 0 

0 0 

0 0 


0 0 0 0 

-2cf[Avf s x]/A/ 0 0 0 

-[A0® s x]/Af 0 l -l 0 

0 — - 0 0 

r Av 

0 0 — -I 0 

*A 0 

0 0 0 0 


(26) 

(27) 


The 3x3 matrix G^ is derived numerically as the perturba- 
tion of the gravity vector’ s dependence upon vehicle location. 
This matrix compensates for the fact that the gravity models 
change with vehicle position. It is gravity model specific, 
although the method presented here is generic with respect to 
the gravity model chosen. 

Note that a typical noise value is the bias time constants. 
The scalar values x Av and x A0 represent the time constants 
associated with the bias random walk. 


0 

0 

0 


f(r, r + A r) 


= (Ar/ + ^) 


Or 


0 


0 


0 0 
0 0 
0 0 
0 0 


0 1 


(28) 


45 measurements are affected by the carrier phase wind up. 
Carrier phase wind up causes a phase advancement that is 
proportional to the rotation of the GPS antenna. This rotation 
provides extra information to compensate for the yaw bias. As 
described by Hong, et. al., in “Observability Analysis ofGPS 
50 Aided INS,” Proceedings of the 2000 Institute of Navigation, 
Salt Lake City, Utah, September 2000, yaw bias is weakly 
observable when corrected by GPS. 

Some embodiments of the inventive system and method as 
. . described herein use a new measurement model that takes 
advantage of new results in GIPSY (GNSS (Global Naviga- 
tion and Satellite Systems) — Inferred Positioning System 
and Orbital .Analysis Software) software processing, which 
employs a windup model to estimate the attitude of the sat- 
60 ellite spacecraft. As described herein, a wind up model can 
now be used to further compensate an inertial system. 

GPS Measurement Model 

A simplified model is described hereinbelow for the carrier 
phase measurements. The model can be applied to the LI, L2, 
65 frequencies, or the ionosphere free combination LC. This 
model can also be applied to other frequencies such as L5. A 
method known in the art is used to resolve the ambiguity 
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between the satellite and the GPS receiver on the vehicle. 
Such methods have been described, for example, by Bertiger, 
et. al., in “Single Receiver Phase Ambiguity Resolution With 
GPS Data,” Submitted to the Journal of Geodesy. Eq. 31 
defines the error model in terms of the state error. Note that <j> ( 5 
represents the carrier phase from satellite i to the GPS receiver 
and no longer represents the roll angle when defined in Eq. 1 0. 
From this point forward, <|) will be used explicitly for the 
carrier phase. The roll angle is assumed to be part of the vector 
Y- 10 

X(^+7V)=p,+/f z 6x+<|) X j-<t> ic +'n i (31) 

The measurement model for the carrier phase measure- 
ment (j), in Eq. 31 lumps ionosphere and troposphere errors 15 
into the noise term r| for simplicity of the measurements. 
There are multiple ways for estimating and removing the 
ionosphere and troposphere estimates such as using the iono- 
sphere free carrier phase combination as for example, have 
been described by Blewitt, et. al, in “Carrier Phase Ambiguity 20 
Resolution for the Global Positioning System Applied to 
Geodetic Baselines up to 2000 km,” Journal of Geophysical 
Research, Vol. 94, No. B8, Pages 10, 187-10, 203, Aug. 10, 
1989, or by Blewitt, et. al., in “Carrier Phase Ambiguity 
Resolution for the Global Positioning System Applied to 
Geodetic Baselines up to 2000 km,” Journal of Geophysical 
Research, Vol. 94, No. B8, Pages 10, 187-10, 203, Aug. 10, 
1989, and then estimating the tropospheric conditions, as has 
been described by Bar-Sever, et. al., in “Estimating horizontal 30 
gradients of tropospheric path delay with a single GPS 
receiver,” Journal of Geophysical Research, Vol. 1 03, No. B3, 
Pages 5019-5035, Mar. 10, 1998. The subscript i refers to the 
GPS satellite number and the subscript x refers to errors 
associated with the vehicle state. The integer ambiguity N, is 35 
estimated using a separate filter such as has been described by 
Bertiger, et. al. 

The term (|) T1 . refers to the carrier phase wind up term at the 
vehicle’s GPS receiver caused by satellite i. In addition to the 
other terms, the present description defines how errors in the 40 
vehicle state contribute to the errors in Similarly, the GPS 
satellite transmitter has a phase wind up (j> iT which is associ- 
ated with the motion of the GPS satellite. The present descrip- 
tion assumes that the satellite wind up is fixed, known through 
an outside estimation process such as GIPSY, or transmitted 4 - 
to the vehicle for use in data processing. 

GPS Range Based Error Model 

The true range and clock biases are lumped together into p, 
in Eq. 32. The true values are a function of the estimated range 
and clock p ; . and errors due to the state estimate error 8x. The 50 
range estimate p, is defined in Eq. 33 in which P/' is the 
estimate of the GPS satellite position and ct ; . is the estimate of 
the GPS clock bias . The position of the vehicle is the center 

of the IMU instrument. The lever ann defines the 

location of the GPS antenna phase center relative to the center 55 
of the IMU in the vehicle body frame. It is assumed that this 
lever arm is known, calibrated and fixed throughout the 
motion of a vehicle or platform (e.g. an aircraft flight). How- 
ever, error models can be implemented to include errors in 


this lever ann. 



(32) 

Pi=\^?-I^-T B E L afu B \\+circi 

(33) 


Hie measurement sensitivity matrix H„ defined in Eq. 34, 
refers to perturbations with respect to the state errors. 


Hi = 


||7f - ? E - cfzV || 


[ / 0 cf [ t.JU 


x] 0 0 1 ] 


( 34 ) 


The line of sight vector from the GPS antenna to the GPS 
satellite as in Eq. 35, can be simplified for the following 
development: 


(/f-pE-C^V) 
||7f -c^-VH 


(35) 


Now turning to “observability”, the measurement sensitiv- 
ity matrix defined in Eq. 34 is weakly observable. The issue of 
observability centers around the fact that even though posi- 
tion is fully observable and all of the other errors such as 
velocity and attitude integrate into position, the attitude is 
best observed through the lever ann h IML f. However, the 
sensitivity matrix is unobservable along any attitude vector 
error parallel to L /Afi7 s , because of the cross product tenn in 
Eq. 34. Therefore, there is always a lack of direct observabil- 
ity in attitude along that line which can be estimated, when 
possible, through integration of attitude error into velocity 
and then into position through the error dynamics in Eq. 27. 
This integration uses acceleration of the vehicle in order to 
enable the attitude errors to propagate into velocity. Without 
acceleration, the errors do not propagate and the full state is 
unobservable. Hong, et. al., in “Observability Analysis of 
GPS Aided INS,” Proceedings of the 2000 Institute of Navi- 
gation, Salt Lake City, Utah, September 2000, describe add- 
ing multiple GPS antennas separated by different lever arms 
to enable observability of the complete attitude error state. 
Some embodiments of the instant invention achieve the full 
state observability by using a wind up model with only a 
single GPS antenna. 

Also, while Bertiger typically estimates the integer ambi- 
guity which will include the modulo 2jt wind up angle, the 
description herein estimates a complete phase wind up angle 
without the modulo process. The phase wind up accumulates 
over time and (j> TI is a variable that is book-kept during the 
entire navigation process on a satellite by satellite basis. Simi- 
larly, the accumulated value for <|) JX is also book kept. 

Wind Up Error Model 

In Eq. 3 1 , the phase wind up term due to the yaw motion of 
the satellite c)) IX , and the phase wind up term due to the motion 
of the vehicle ij> xi were introduced. The dipole vector equation 
is given in Eq. 37. The method uses effective dipoles repre- 
senting the antenna at the receiver and the antenna at the GPS 
satellite. .An antenna coordinate frame is defined such the 1 
axis points in the direction of the dipole whose signal is used 
directly, the J axis points in the direction of the dipole signal 
that is delayed by 90 degrees and the E axis completes the 
right handed coordinate system. Then a dipole vector can be 
constructed to describe the effective dipole angle rotated into 
a plane perpendicular to the line of sight vector H^.^ of Eq. 
35 represented in the ECEF frame. A known rotation matrix 
C G ‘ rotates from the GPS antenna frame to the IMU frame. 
The unit vectors i, j, and k are also assumed known in the 
antenna reference frame. The rotation C B E rotates from the 
IMU frame to the ECEF frame relating GPS position esti- 
mates to the antenna frame. 




— Gy' - ' it TT 

~Cr Czr £!,„ 


(36) 




( 37 ) 
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As described in Wu, the first two terms of Eq. 37 represent 
the projection of the x axis onto a plane perpendicular to 
H Ios G and the third term represents the y-axis onto the same 
plane rotated by 90 degrees. The effective wind up angle is 
defined using the wind up vector as shown in Eq. 39. 


d x 

dy 


d z 


( 38 ) 


10 



( 39 ) 


A similar relationship holds for the satellite effective wind 
up. d/ in Eq. 40 is defined for unit vectors T„ j„ and k, repre- 
senting the unit directions in the satellite broadcast antenna 
dipole. Unlike Wu, the effect on wind up on the vehicle and 
the satellite are defined here separately, arguing that the sat- 20 
ellite errors are known and using the vehicle wind up to 
correct the inertial heading angle. 


4 ' 

4= 4 

A. 


( 40 ) 25 

( 41 ) 

30 


The satellite phase advance due to the dipole rotation is 
therefore: 


(p lx = tani 



35 


( 42 ) 


The error model for the terrestrial vehicle is now defined in 
terms of the navigation state and error states previously 
defined. First, the dipole equation in Eq. 37 is multiplied by 
the range in Eq. 43 to form a new, equivalent dipole equation 
in Eq. 44. 

/ ! = ||7f-p £ -cf4„,|| 

Dp = h df 

= hi - hHg. ((//£,. ) r 7) - hHg. x 7 
= w - ((C?Cgtf&. ) r ?)(/f - P £ - CfLjU,) + 

(Pf -r* -c%Ll MU )x7 

Using the function in Eq. 43 to normalize the dipole vector 
makes several assumptions about the linearity of the system 
and neglects higher order terms. Elowever, the range is domi- 
nated by the relative position PT-p /; p or terrestrial applica- 60 
tions, the relative position vector has a norm of about 2.0x10 
meters and errors in each axis are expected to be less than a 
meter. Therefore, uncertainty in position contributes less than 
1.0x10“ '. This error contributes only mildly to estimation 
error. 65 

Perturbations about the errors in vehicle position P /; and 
attitude C B are now described as performed previously. A 


45 

( 43 ) 

( 44 ) 

50 

55 
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Modified Gain Extended Kalman Filter is then used to relate 
these perturbations to the wind up angle (|>. 


‘It. ‘It. * H 7A t (45) 

First the perturbations in the D, G vector by substituting in 
the perturbations for the position and attitude estimates, com- 
bining terms, and eliminating terms of order greater than 2. 


Dp = Dp + <5Dp 
= Dp + H d 6x 


(46) 


The a priori estimate DA is defined in Eq. 47. The sensi- 
tivity matrix H^, is defined in Eq. 5 1 , with higher order per- 
turbations neglected. 


of = hi - {{C?(f E Hg. ) 7)(c?cf (pf -A)- dfL B mu ) + 

(c?cf(pf -A)-CgL B MU )xJ 


( 47 ) 



( 48 ) 


The translation from 6D, G is performed in the following 
steps where perturbation terms of order greater than one have 
been neglected: 


<5 D, = -{(H E ) T A B [Sy xic'p) [cfcf (pf - P £ ) + dfL B MU ] - 
{(H £ ) r cfcy)cf cf [SPf -61*) - 


{(W £ ) r cfcy}[cfMrx]cf(pf -p £ )] + 

cfcfospf - 5P 2 ) X 7 + Cf [-6r x]cf (pf - P 2 ) X 7 


Noting that (a 7 bl||a||)a=(a 7 b)al||a||, and that [ax]b=-[bx]a, 
we can then reduce line by line, separating out the perturba- 
tion terms. 


6D, = -{[cfc£(pf - P E ) + cfz.V] r ilU[(cfw £ )x]<Sr- (50) 

|(D £ ) £ cf C^7}cf cf (6Pf -<5P £ ) - 
{(// £ ) 7 cfc(;J}[cf[(cf(7f -A))x]6y + 
cfcf[-7x](6/f -6P £ )- cf [([(cf(7f -P £ ))x]7)x]5r 

If we ignore errors in the satellite geometry 8P, £ then we 
can reduce SD^H^Sx where H^, is defined in Eq. 5 1 . 

H d = [Hd p Txj H D y 0 3x 3 0 3x3 0 ] (51) 

Hop = -{(D £ ) r cfc'7jcfcf +cfrf[-7x] < 52 ) 
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( 64 ) 


-{[e?cf(7f - 7*) + cfzvfijcf [(4« £ )x] -{(Htfc^c'cl} 
cf [(CS(7f - 7*))X] - cf [(pS(jf - T'JxjTjx] * 
-2j(H £ ) T C^C' ; 7iCf[(c?(7f-P E ))x]- 

C*(jf-P>]7H 


Perturbations can be taken with respect to D v and D v as shown 
below. 


Note that there are two cross product matrix terms in Eq. 53 
which define the observability of the attitude. Attitude is 
unobservable along the line of sight vector in the first term, 
and unobservable along the cross product of the line of sight 
vector and the J axis. In which ever direction the line of sight 
points, the error in the z axis of the attitude error 6 y is observ- 
able. However, the error model is incomplete since one goal is 
to estimate the error in SP® and by using the wind up error (|) t . 

Eq. 45 uses a form of the MGEKF (Modified Gain 20 
Extended Kalman Filter) as described by Wolfe, et. al., in 
“Target Association Using Detection Methods,” Journal of 
Guidance, Control and Dynamics, Vol. 25, No. 6 , November- 
December 2002, fortarget tracking. The solution uses a trans- 
formation of variables in the following steps. Using the rela- 23 
tionship of Eq. 54, it is possible to rewrite Eq. 45 into Eq. 59 
using the newly formed residual in Eq. 55. 


( D x {dD y ) - D y (dD x ) 


Therefore, the error can be written in the form in Eq. 66 with 
H j defined in Eq. 67, H s defined in Eq. 68 and Hq defined 
previously in Eq. 5 1 . 


<Pxi-<P x , = H A H b H d 6x 


tan l {a)-tan l (b) = tan ^ + 

r; = A(7; 4- /V;) - p ; - (j> xi - <j) ix = tan~ l (4) 

( 3 ; = A( J; + (Vi) - Pi - f jx 


/•; = (8; - /?; = tan -1 ot) 


- Flan 1 (4) 


[sin(3;) - cos(ySy) 0 ]//d6.v 


[cos(y3)D, + sin(ft)D y 


Therefore, three matrices define the relationship of the 
error in the state estimate 8 x on the phase wind up of the 
vehicle, (jU 


: ^ MGEKF H<pi bi[)Sx 


-Fran -1 (S') 


/A = [ sin(/t;t -COS (y§ ; ) 0] 


( 54 ) 30 Thus, it can be seen that the direct form presented here is 
very similar in form to the MGEKF. 

(J5 ) Total Error Model 

We now combine the error models from all three sources: 

(56) 35 range, yaw wind up, and geometry wind up. Also, the basic 

model with Eq. 69, and each of the definitions of the a priori 

( 57 ) terms are repeated. 

(58) X(4+A6)=p,+4„-$ to +7ft»:+ri, (69) 

40 — 

(5g) The a priori range py is defined in Eq. 70 with variables 

defined previously. 


p i =||/ J i £ -F £ -C/Z, / M£/ fi |!+«rcr (70) 

The a priori wind up due to the vehicle’s GPS antenna 
rotation is defined in Eq. 7 1 . An initial wind up angle <|> X! (to) is 
defined at the beginning of the phase arc. This initial condi- 
tion can be defined as the current estimate of the phase wind 
up given the vehicle attitude. 

U=(fc(0-k,tto)) (71) 

The a priori wind up due to the vehicle’s satellite antenna 
rotation is defined in Eq. 72 where § ix is the satellite wind up 
of the antenna and defined in Eq. 74 with initial condition 
ftxOo)- 




Alternative Wind Up Model 

While the MGEKF works best when the residual is purely 
an angular system, as described hereinbelow, the MGEKF 
can be used in a slightly different application. In the present 
case, the measurement residual is partly range based and 6 5 
partly angular based. A more direct approach, however, pro- 
duces similar results. 
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The sensitivity matrix H is defined in Eq. 75 with H, 
defined in Eq. 34, ^ MGEKF defined in Eq. 62, H^. defined in 
Eq. 63, H^, defined in Eq. 5 1 . The state error for the vehicle is 
defined in Eq. 25. 

H=H,+H MGEKF H^fl D (75) 

A similar model can be constructed to estimate the yaw 
angle of the GPS satellite and the effect on wind up 
However, this is neglected at this point because the GPS 
satellites do not transmit yaw angle and other means are 
available within GIPSY to estimate the satellite yaw angle as 
described by Bar-Sever, et. al., in “Fixing the GPS Bad Atti- 
tude: Modeling GPS Satellite Yaw During Eclipse Seasons,” 
Navigation, Vol. 43, NO 1, Spring 1996. Therefore, for the 
present case, <() !X is assumed fixed, known precisely, or esti- 
mated through another estimation scheme. With a full set of 
static ground stations, the method presented here could be 
used to estimate the attitude of any or all of the GPS constel- 
lation as well as the position. 

An Extended Kalman Filter to estimate the errors in the 
state Sx using the measurements in Eq. 69 and the dynamics 
defined in Eq. 26 can be formed. Correction of the state with 
the estimated state error takes place as has been described by 
Williamson, et. al., in “Sensor Fusion Applied to Autonomous 
Aerial Refueling,” Journal of Guidance, Control, and Dynam- 
ics, Vol. 32, No. 1, January-February 2009. 

Filtering 

Using the error model for the strap down equations of 
motion described hereinabove in the section Perturbations 
and Error Propagation, and the measurement model in the 
section Total Error Model, an Extended Kalman Filter (EKF) 
or Square Root Information Filter (SRIF) which can be used 
to correct the navigation state generated using the strap down 
equations of motion for errors due to the accelerometer and 
gyro biases is now described. The filter proceeds according to 
the following steps: 

Propagating the initial estimate of the state in Eq. 9 forward 
in time by At using inertial measurements of acceleration and 
angular rate through the strap down equations of motion 
defined as described hereinabove in the section Strap Down 
Equations of Motion. 

Propagating the initial error covariance M forward in time 
using Eq. 30. 

P(t+kt)=<&(t,t+kt)MQ\t,t+/i.t)+T(t,t+kf) WT t (t,t+/S.t) (30) 

In this case W is the process noise associated with the error 
model for the IMU, t is time, At is the time increment, €> is the 
linearized state transition matrix defined in Eq. 27, and T is 
the linearized matrix defined in Eq. 28. 

Repeating the first two steps until a GPS measurement is 
available and the state estimate is time aligned with the GPS 
measurement. 

Forming the code and carrier phase residuals for each 
satellite i using the current best estimate of the state: 

r 0l .=Mi+V)-p, (7 6) 

r p =PrP, (77) 

Calculating (j) T1 as shown in Eq. 39 and 4>, v as shown in Eq. 
42. 

Calculating the value of p, as shown in Eq. 33. 

Calculating the measurement sensitivity matrix H as 
shown in Eq. 75. 

Updating the covariance matrix M 

M=[P+H r V~ v H]~ 1 (78) 

In this case V represents the measurement covariance of the 
code and carrier phase measurements. 


Calculating the gain matrix K as: 

k=ph t v- 1 (79) 

Calculating the correction to the navigation state 8x as: 

5 

8x=Kr (81) 

in this case, r is the column vector of all available code and 
carrier phase residuals r^ and r p at time t. 

Applying the calculated correction 8x to the navigation 
to state x using the method described by Williamson, et. al., in 
“Sensor Fusion Applied to Autonomous Aerial Refueling, 
Journal of Guidance, Control, and Dynamics,” Vol. 32, No. 1, 
January-February 2009 

Repeating this process propagating through the next set of 
15 inertial data until the next set of GPS data is available 
Observability 

As described by Hong, et. al, in “Observability Analysis of 
GPS Aided INS,” Proceedings of the 2000 Institute of Navi- 
gation, Salt Lake City, Utah, September 2000, GPS/INS sys- 
20 terns are susceptible to errors related to a lack of direct 
observability. We now describe how the wind up model 
ensures that the system is completely observable in attitude as 
well as position (full state observability). 

The observability can be reduced to a rank test of the 
25 following matrix which includes the attitude sensitivity of Eq. 
34 and the two cross product terns of the wind up error model 
sensitivity to attitude in Eq. 53. The condensed observability 
matrix in Eq. 76 shows that the first term is observability to all 
errors in 8y except errors along the lever arm vector L IMU B . 
30 The second term shows that errors are observable except 
along the line of sight vector from the receiver to the satellite 
in the antenna frame. The third terni is observable in all axes 
except cross product of the line of site vector crossed with the 
antenna J axis. 

35 

H^=V/[L !Mu ^J + C I °[(r/(Pf--p E ))H]+ 

C/ G [((C e b (P b -P e ))xj)x] (82) 

rank(H obs )>=3 (83) 

40 Consider a typical installation in which the GPS antenna is 

directly above an IMU by about 1 meter and where the IMU 
and GPS coordinate frames are co-incident (accounting for 
the rotation from fore-starboard-down to fore-port -up). Then 
the first term in Eq. 76 is unobservable about the vertical axis, 
45 which is parallel to the lever arm. Assuming a worst case 
where the satellite was directly overhead, the second term is 
also unobservable about the z axis since the line of sight 
vector is parallel to the z axis. However, the third term for a 
satellite directly overhead is unobservable in the x-axis only ( 
50 Kxj=-I). The y and z axes are observable through this term. 
Therefore, the system is completely observable. While spe- 
cial cases may arise which prohibit observability for a par- 
ticular satellite along a particular line of sight vector, the 
plurality of GPS satellites will ensure observability of the full 
55 attitude state. 

Differential GPS 

The methods described herein are applicable to short base- 
line GPS solutions in which the single or double differenced 
carrier phase measurements are used to provide a relative 
60 range between two vehicles. Wide lane carrier phase is typi- 
cally used as a preferred measurement, since it does not 
exhibit the effect of phase wind up. The measurement model 
presented here can be used to enhance the previous methods 
and enable LI, L2 or ionosphere free estimation. If the dif- 
65 ference is made relative to a static ground station, the method 
can be further simplified, since the ground station does not 
have any change in attitude and would not likely need an 
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inertial system. The satellite common mode phase wind up is 
removed from the single differenced measurements. 
Estimation of Satellite Variables 

As described hereinabove, the satellite transmitter anten- 
nas have a known location, a known clock bias, and a known 
antenna attitude. These values can be provided by a high 
fidelity model such as the one developed in GIPSY. However, 
it is possible to replace the tenns associated with the GPS 
receiver with the transmitter in order to estimate a transmitter 
location precisely. The problem is that one GPS receiver on 
the ground cannot be used to estimate the entire constellation 
of GPS satellites. However, multiple GPS receivers, such as 
the network of ground based receivers designed to support 
GIPSY can be used to generate enough measurements to 
make this model feasible. It is contemplated that the inventive 
method described herein can provide an enhancement to the 
existing attitude system in GIPSY. 

The Estimation of Attitude Using Multiple Antennas 

It has been common in GPS to utilize multiple GPS anten- 
nas for attitude. However, the basic scheme used is to use 
range measurements or estimate the integer ambiguity 
between two or more GPS receiver antennas located on a 
common vehicle. It is generally assumed that the antennas are 
rigidly spaced with a reasonable baseline. Since the baseline 
is precisely known to a few centimeters in the ECEF reference 
frame, a lever arm of 1 meter between GPS antennas could be 
used to provide an attitude estimate with accuracies on the 
order of 0.01 radians, or 0.5 degrees. The longer the baseline, 
the more accurate the attitude. 

The method described herein suggests an alternative 
approach. In addition to taking advantage of the integer wind 
up to make the entire attitude estimate observable, it is pos- 
sible to further enhance this approach using multiple anten- 
nas. However, instead of using a long baseline, the antemias 
could be placed next to each other, but at different angles. 
Different angles would make sure that each antenna error 
model would have a different dipole definition. Common 
satellites in view affect the antennas differently. A multiple 
antenna element design where the dipole vectors were ampli- 
fied or exagerated in I over] (not radially symmetric) could be 
used to help identify the initial condition. Multiple elements 
could also be set in a small circle with only a few centimeters 
of baseline separation and used to provide complete heading 
information as well as pitch and roll information without 
having to resort to a longer baseline. Such a multiple antenna 
system can use a separate set of RF front ends on the GPS 
receiver for each antenna with carrier phase tracking loops on 
each antenna. The result can be a small, solid state device 
capable of measuring vehicle attitude from essentially a small 
antenna foot print. 

DEFINITIONS 

Recording the results from an operation or data acquisition, 
such as for example, recording results at a particular fre- 
quency or wavelength, is understood to mean and is defined 
herein as writing output data in a non-transitory manner to a 
storage element, to a machine-readable storage medium, or to 
a storage device. Non-transitory machine-readable storage 
media that can be used in the invention include electronic, 
magnetic and/or optical storage media, such as magnetic 
floppy disks and hard disks; a DVD drive, a CD drive that in 
some embodiments can employ DVD disks, any of CD-ROM 
disks (i.e., read-only optical storage disks), CD-R disks (i.e., 
write-once, read-many optical storage disks), and CD-RW 
disks (i.e., rewriteable optical storage disks); and electronic 
storage media, such as RAM, ROM, EPROM, Compact Flash 
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cards, PCMCIA cards, or alternatively SD or SDIO memory; 
and the electronic components (e.g., floppy disk drive, DVD 
drive, CD/CD-R/CD-RW drive, or Compact Flash/PCMCIA/ 
SD adapter) that accommodate and read from and/or write to 
5 the storage media. Unless otherwise explicitly recited, any 
reference herein to “record” or “recording” is understood to 
refer to a non-transitory record or a non-transitory recording. 

As is known to those of skill in the machine-readable 
storage media arts, new media and formats for data storage 
to are continually being devised, and any convenient, commer- 
cially available storage medium and corresponding read/ 
write device that may become available in the future is likely 
to be appropriate for use, especially if it provides any of a 
greater storage capacity, a higher access speed, a smaller size, 
15 and a lower cost per bit of stored information. Well known 
older machine-readable media are also available for use under 
certain conditions, such as punched paper tape or cards, mag- 
netic recording on tape or wire, optical or magnetic reading of 
printed characters (e.g., OCR and magnetically encoded sym- 
20 bols) and machine-readable symbols such as one and two 
dimensional bar codes. Recording data for later use (e.g., 
writing data to memory or to digital memory) can be per- 
formed to enable the use of the recorded information as out- 
put, as data for display to a user, or as data to be made 
25 available for later use. Such digital memory elements or chips 
can be standalone memory devices, or can be incorporated 
within a device of interest. “Writing output data” or “writing 
data to memory” is defined herein as including writing trans- 
formed data to registers within a microcomputer. 

30 “Microcomputer” is defined herein as synonymous with 
microprocessor, microcontroller, and digital signal processor 
(“DSP”). It is understood that memory used by the micro- 
computer, including for example instructions for data pro- 
cessing coded as “firmware” can reside in memory physically 
35 inside of a microcomputer chip or in memory external to the 
microcomputer or in a combination of internal and external 
memory. Similarly, analog signals can be digitized by a stan- 
dalone analog to digital converter (“ADC”) or one or more 
ADCs or multiplexed ADC channels can reside within a 
40 microcomputer package. It is also understood that field pro- 
grammable array (“FPGA”) chips or application specific inte- 
grated circuits (“ASIC”) chips can perform microcomputer 
functions, either in hardware logic, software emulation of a 
microcomputer, or by a combination of the two. Apparatus 
45 having any of the inventive features described herein can 
operate entirely on one microcomputer or can include more 
than one microcomputer. 

General purpose programmable computers useful for con- 
trolling instrumentation, recording signals and analyzing sig- 
50 nals or data according to the present description can be any of 
a personal computer (PC), a microprocessor based computer, 
a portable computer, or other type of processing device. The 
general purpose programmable computer typically includes a 
central processing unit, a storage or memory unit that can 
55 record and read information and programs using machine- 
readable storage media, a communication terminal such as a 
wired communication device or a wireless communication 
device, an output device such as a display terminal, and an 
input device such as a keyboard. The display terminal can be 
60 a touch screen display, in which case it can function as both a 
display device and an input device. Different and/or addi- 
tional input devices can be present such as a pointing device, 
such as a mouse or a joystick, and different or additional 
output devices can be present such as an enunciator, for 
65 example a speaker, a second display, or a printer. The com- 
puter can run any one of a variety of operating systems, such 
as for example, any one of several versions of Windows, or of 
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MacOS, or of UNIX, or of Linux. Computational results 
obtained in the operation of the general purpose computer can 
be stored for later use, and/or canbe displayed to a user. At the 
very least, each microprocessor-based general purpose com- 
puter has registers that store the results of each computational 5 
step within the microprocessor, which results are then com- 
monly stored in cache memory for later use. 

Many functions of electrical and electronic apparatus can 
be implemented in hardware (for example, hard- wired logic), 
in software (for example, logic encoded in a program operat- to 
ing on a general purpose processor), and in firmware (for 
example, logic encoded in a non-volatile memory that is 
invoked for operation on a processor as required). The present 
invention contemplates the substitution of one implementa- 
tion of hardware, firmware and software for another imple- 15 
mentation of the equivalent functionality using a different one 
of hardware, firmware and software. To the extent that an 
implementation can be represented mathematically by a 
transfer function, that is, a specified response is generated at 
an output terminal for a specific excitation applied to an input 20 
terminal of a “black box” exhibiting the transfer function, any 
implementation of the transfer function, including any com- 
bination of hardware, firmware and software implementa- 
tions of portions or segments of the transfer function, is con- 
templated herein, so long as at least some of the 25 
implementation is performed in hardware. 

Theoretical Discussion 

Although the theoretical description given herein is 
thought to be correct, the operation of the devices described 
and claimed herein does not depend upon the accuracy or 30 
validity of the theoretical description. That is, later theoretical 
developments that may explain the observed results on a basis 
different from the theory presented herein will not detract 
from the inventions described herein. 

Any patent, patent application, or publication identified in 35 
the specification is hereby incorporated by reference herein in 
its entirety. Any material, or portion thereof, that is the to be 
incorporated by reference herein, but which conflicts with 
existing definitions, statements, or other disclosure material 
explicitly set forth herein is only incorporated to the extent 40 
that no conflict arises between that incorporated material and 
the present disclosure material. In the event of a conflict, the 
conflict is to be resolved in favor of the present disclosure as 
the preferred disclosure. 

While the present invention has been particularly shown 45 
and described with reference to the preferred mode as illus- 
trated in the drawing, it will be understood by one skilled in 
the art that various changes in detail may be affected therein 
without departing from the spirit and scope of the invention as 
defined by the claims. 50 

What is claimed is: 

1. A method of stabilizing an inertial navigation system 
(INS), comprising the steps of: 

performing the following steps in any order: 

receiving data from an inertial navigation system having 55 
an inertial measurement unit (IMU); and 
receiving a finite number of carrier phase observables 
using at least one GPS receiver from a plurality of 
GPS satellites; 

performing the following series of calculations: 60 

calculating a phase wind up correction; 
correcting at least one of said finite number of carrier 
phase observables using said phase wind up correc- 
tion; and 

calculating a corrected IMU attitude or velocity or posi- 65 
tion using said corrected at least one of said finite 
number of carrier phase observables; and 
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performing a step selected from the steps consisting of 
recording said corrected IMU attitude or velocity or 
position, reporting said corrected IMU attitude or veloc- 
ity to a user, and providing said corrected IMU attitude 
or velocity or position to another process that uses the 
corrected IMU attitude or velocity or position. 

2. The method of stabilizing an inertial navigation system 
of claim 1, further comprising the step of providing said 
corrected IMU attitude or velocity or position to said INS. 

3. The method of stabilizing an inertial navigation system 
of claim 1, wherein said step of calculating a corrected IMU 
attitude or velocity or position includes calculating a cor- 
rected IMU attitude or velocity or position using said cor- 
rected at least one of said finite number of carrier phase 
observables with full state observability. 

4 . The method of stabilizing an inertial navigation system 
of claim 1, wherein said steps of calculating a phase wind up 
correction and correcting at least one of said finite number of 
carrier phase observables using said phase wind up correction 
are performed using the following equations: 



and 


<Pxi ~ = H MG EKFHfjH D 6 x , 

where 

-Ftan l (' P) 

Hmgekf = . 

and 

% = [sin(/() - cos(/J, 4 0], 

5. The method of stabilizing an inertial navigation system 
of claim 1, wherein said steps of calculating a phase wind up 
correction and correcting at least one of said finite number of 
carrier phase observables using said phase wind up correction 
are performed using the following equations: 


and 

<f> xi -t xi = H A H B H D 6x, 
where 


and 


6. The method of stabilizing an inertial navigation system 
of claim 1, wherein said step of receiving a finite number of 
carrier phase observables comprises receiving a finite number 
of carrier phase observables using at least one GPS receiver 
from a plurality of GPS satellites using a GPS receiver having 
a plurality of GPS antennas disposed at different angles with 
different dipole definitions in a coordinate reference system 
and wherein said step of performing the following series of 
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calculations includes calculating a phase wind up correction 
for each of said plurality of GPS antennas disposed at differ- 
ent angles. 

7. The method of stabilizing an inertial navigation system 
of claim 1, wherein said step of receiving a finite number of 5 
carrier phase observables comprises receiving a finite number 
of carrier phase observables using at least one GPS receiver 
from a plurality of GPS satellites using a plurality of GPS 
antennas disposed substantially in a circle and wherein said 
step of performing the following series of calculations to 
includes calculating includes calculating a phase wind up 
correction for each of said plurality of GPS antennas disposed 
substantially in a circle. 

8. The method of stabilizing an inertial navigation system 
of claim 1, wherein a GPS receiver having at least one GPS 15 
antenna is present on a first vehicle, and a second GPS 
receiver having at least one GPS antenna is configured to 
perform the method of claim 1 on a second vehicle different 
from said first vehicle, and further comprising a step of using 

a single or double differenced carrier phase measurement to 20 
provide a range between said first and said second vehicles. 

9. The method of stabilizing an inertial navigation system 
of claim 1, wherein said step of calculating a corrected IMU 
attitude or velocity or position provides an enhanced accuracy 
yaw angle. 

10. The method of stabilizing an inertial navigation system 
of claim 1, wherein said step of calculating a corrected IMU 
attitude or velocity or position includes calculation of an 
Extended Kalman Filter (EKF) or of a Square Root Informa- 
tion Filter (SRIF). 

11. A GPS stabilized inertial navigation system apparatus 
comprising: 

at least one GPS receiver having at least one GPS antenna; 

an inertial navigation system (INS) having an inertial mea- 
surement unit (IMU); 

a data processor communicatively coupled to said at least 
one GPS receiver and said IMU, said data processor 
configured to receive a plurality of GPS carrier phase 
observables from said GPS receiver and attitude and 
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velocity data from said IMU, said data processor includ- 
ing a module having instructions in machine-readable 
form recorded therein, said data processor configured to 
perform, when operating under the control of said 
instructions, a process comprising the steps of: 
calculating a phase wind up correction; 
correcting at least one of said finite number of carrier 
phase observables using said phase wind up correc- 
tion; and 

calculating a corrected IMU attitude or velocity or posi- 
tion using said corrected at least one of said finite 
number of carrier phase observables; and 
at least one device selected from the group of devices 
consisting of a device configured to record said cor- 
rected IMU attitude or velocity or position, a device 
configured to report said corrected IMU attitude or 
velocity or position to a user, and a device configured to 
provide said corrected IMU attitude or velocity or posi- 
tion to another process that uses the corrected IMU 
attitude or velocity or position. 

12. The GPS stabilized inertial navigation system appara- 
tus of claim 1 1 , wherein said INS is configured to receive said 
corrected IMU attitude or velocity or position to correct an 
INS attitude or velocity or position. 

13. The GPS stabilized inertial navigation system appara- 
tus of claim 11, wherein said data processor is configured to 
calculate a corrected IMU attitude or velocity or position with 
Hill state observability. 

14. The GPS stabilized inertial navigation system appara- 
tus of claim 11, further comprising a plurality of GPS anten- 
nas disposed at different angles. 

15. The GPS stabilized inertial navigation system appare- 
nt s of claim 11, further comprising a plurality of GPS anten- 
nas disposed substantially in a circle. 

16. The GPS stabilized inertial navigation system appara- 
his of claim 11, wherein said data processor is configured to 
provide an enhanced accuracy yaw angle. 



